#include "WPILib.h"
#include "KHSJoystick.h"
#include "GearBox.h"
#include "Jaguar.h"
#include "Encoder.h"
#include "Relay.h"
#include "DigitalOutput.h"
#include "DriveSystem.h"
#include "vision/PCVideoServer.h"
#include "Victor.h"
#include "CameraSystem.h"
#include "DashboardDataFormat.h"
#include "DeploymentSystem.h"
#include "LineSensorArray.h"
#include "AutonomousSystem.h"






/**
 * This is a demo program showing the use of the RobotBase class.
 * The SimpleRobot class is the base of a robot application that will automatically call your
 * Autonomous and OperatorControl methods at the right time as controlled by the switches on
 * the driver station or the field controls
 *
 *
 */
class RobotDemo : public SimpleRobot
{
	KHSJoystick *mptr_joystick;
	DriveSystem m_driveSystem;
	Gamepad *gamepad;
	Timer *timer;
	CameraSystem m_cameraSystem;
	ManipulatorSystem m_manipulatorSystem;
	Compressor m_compressor;
	DeploymentSystem* mptr_deploymentSystem;
	
	AutonomousSystem m_autonomousSystem;
	bool m_manipByGlove;



public:
	RobotDemo(void):m_driveSystem(), m_cameraSystem(), m_manipulatorSystem(),m_compressor(4,8,4,2),
	m_autonomousSystem(&m_driveSystem, &m_manipulatorSystem)
	{
		printf("Begin MyRobot Constructor\n");


		GetWatchdog().SetExpiration(400);
		
		mptr_deploymentSystem = new DeploymentSystem();
		m_manipByGlove = false;
		mptr_joystick = new KHSJoystick(2);
		gamepad = new Gamepad(1);
		timer = new Timer();
		
		printf("Finished constructing MyRobot Object!!\n");
	}
	
	
	void StartCompetition()
	{

		m_cameraSystem.initCamera();
		printf("Competition started");
		m_compressor.Start();

		while (1)
		{
			if (IsAutonomous())
			{
				Autonomous();


			}

			else if (IsOperatorControl())
			{
				OperatorControl();
			}
		}
	}

	void Autonomous(void)
	{
		printf("Starting Autonomous Mode!!\n");
		
		//TODO: convenient way to choose distance/ left right preferences and height.
		float distance = 0;
		bool prefersRight = true;
		float J1 = 90;
		float J2 = 20;
		m_manipulatorSystem.setClamped(true);
		

		//TODO:AUTO Follow Line up to wall while raising arm up to 8ft
		bool isCompleted = false;
		while(IsAutonomous() && !isCompleted)
		{
			bool tmp =false;
			isCompleted = m_autonomousSystem.followLine(200,prefersRight);
			m_autonomousSystem.raiseArm(591,508);
			
		}
		m_autonomousSystem.disable();
		m_autonomousSystem.raiseArm(501,510);
		m_manipulatorSystem.setClamped(false);

		//TODO:AUTO Push the last arm joint forward just a little to place the tube
/*		isCompleted = false;
		while(IsAutonomous() && !isCompleted)
		{
			isCompleted = m_autonomousSystem.raiseArm(501,510);
			
		}
		
		
		//TODO:AUTO Open claw
		isCompleted = false;
		while(IsAutonomous() && !isCompleted)
		{
			m_manipulatorSystem.setClamped(false);
			Wait(1.0);
			isCompleted = true;
		}
		*/
		Wait(1);
		m_manipulatorSystem.disable();
		/*
		//TODO:AUTO drop arm
		isCompleted = false;
		while(IsAutonomous() && !isCompleted)
		{
			isCompleted = m_autonomousSystem.raiseArm(J1-30,J2+10);
		}
	
		//TODO:AUTO retract arm
		isCompleted = false;
		while(IsAutonomous() && !isCompleted)
		{
			isCompleted = m_autonomousSystem.raiseArm(J1-60,20);
		}
		
		//TODO:AUTO turn away from wall ???maybe
		isCompleted = false;
		while(IsAutonomous() && !isCompleted)
		{
			isCompleted = true;
		}
		*/
		//TODO:AUTO drive away.  Maybe in reverse
		isCompleted = false;
		if (IsAutonomous())
		{
			m_driveSystem.drive(-0.5,-0.5);
			Wait(1.0);
			
		}
		if (IsAutonomous())
		{
			m_driveSystem.drive(0.5,-0.5);
			Wait(0.5);
			
		}
		
		

		printf("Finished Autonomous Mode!!\n");
	}

	/**
	 * Runs the motors with arcade steering.
	 */
	void OperatorControl(void)
	{
		printf("Starting OperatorControl Mode!!\n");
		/*
		 * The following line is only for recording waypoints It will leave the bot unable to
		 * be driven.  The waypoints are set by moving the bot by hand and pressing the trigger
		 * to record a point.  This line needs to be commented out in order to drive the bot.
		 */
					//m_autonomousSystem.recordValues(mptr_joystick);
		
		GetWatchdog().SetEnabled(true);


		while (IsOperatorControl())
		{
			
			GetWatchdog().Feed();
			//Wait(0.05);
			//Gives information to the driver station.
			sendIOPortData();
			
			
			//The robot's drive controlled by the gamepad.
			//printf("gpad\n");
			m_driveSystem.drive(gamepad);
			
			//Calibrate the potentiometers on the bot for glove control
			if(mptr_joystick->GetDebouncedButton(6)||mptr_joystick->GetDebouncedButton(7)||
			mptr_joystick->GetDebouncedButton(10)||mptr_joystick->GetDebouncedButton(11))
			{	
				printf("Starting Bot arm calibration\n");
				m_manipulatorSystem.calibrate(mptr_joystick);
			}
			//Calibrate the Potentiometers on the Glove 
			if(mptr_joystick->GetDebouncedButton(8)){
				printf("Starting Arm glove calibration\n");
				m_manipulatorSystem.calibrateArm(mptr_joystick);
			}
			
			else{
				//The robot's arm controlled by the glove.
				//printf(".");
				if(m_manipByGlove){
					m_manipulatorSystem.DriverControlGlove();
				}
				// The robot's arm controlled by joystick
				else{
					m_manipulatorSystem.DriverControlJoystick(mptr_joystick);
				}
			}
			//Toggle between glove control and joystick control.
			if(mptr_joystick->GetDebouncedButton(2)){
				m_manipByGlove = !m_manipByGlove;
			}
			//The camera coordinates is wherever the claw is.
			m_cameraSystem.lookAt(m_manipulatorSystem.GetCameraCoordinates());
			
			//m_autonomousSystem.followLine(5);
			
			//DeploymentSystem
			
			if(gamepad->GetNumberedButton(2)){
				mptr_deploymentSystem->deploy();
			}
			
			else if(gamepad->GetNumberedButton(3)){
				mptr_deploymentSystem->retract();
			}
			
			else{
				mptr_deploymentSystem->stop();
			}
			
			//End DeploySystem

		}
		printf("Finished OperatorControl Mode!!\n");
	}


};

START_ROBOT_CLASS(RobotDemo);

